/*
 * Robot.cpp
 *
 *      Author: Ben, Philippe, Simon en Tom
 */

#include "Robot.h"
#define PI 3.1415


Robot::Robot(){
	this->tolerance_distance = 0;
	this->tolerance_arc = 0;
	this->cargo = false;
}

Robot::Robot(int tol_arc, int tol_distance){
	this->tolerance_arc = tol_arc;
	this->tolerance_distance = tol_distance;
	this->cargo = false;
}

void Robot::UpdateRobot(Package data){
	this->me.x = data.udp_package->info_b.robx;
	this->me.y = data.udp_package->info_b.roby;
	this->me.arc = data.udp_package->info_b.robhoek;
}

void Robot::UpdateTarget(Package data){
	if(!cargo){
		this->target.x = data.udp_package->info_b.doelx;
		this->target.y = data.udp_package->info_b.doely;
		this->target.arc = 0;
	} else {
		this->target.x = data.udp_package->info_b.garx;
		this->target.y = data.udp_package->info_b.gary;
		this->target.arc = 0;
	}
}

Position Robot::getTarget()
{
	return this->target;
}

void Robot::setTarget(Position p)
{
	this->target = p;
}

int Robot::CalcTurn(){
	cout << "this_target.x: " << this->target.x << endl;
	cout << "this_target.y: " << this->target.y << endl;
	cout << "this_me.x: " << this->me.x << endl;
	cout << "this_me.y: " << this->me.y << endl;

	int length_x = this->target.x - this->me.x;
	int length_y = this->target.y - this->me.y;
	cout << "x:" << length_x << " en y:" << length_y << endl;
	float dest_arc = atan((float)(length_y) / (length_x));

	if (length_x < 0 && length_y < 0)
		dest_arc += 2*PI;
	else if(length_x < 0)
		dest_arc += PI;

	cout << "dest_arc_aangepast:" <<dest_arc*180/PI << endl;

	if (length_x == 0)
	{
		if (length_y>0)
			dest_arc = PI/2;
		else
			dest_arc = -PI/2;
	}
	float total_arc = dest_arc*180/PI - this->me.arc;
	if (abs(total_arc) < this->tolerance_arc)
		return 0;
	return (int)total_arc;

}

int Robot::CalcDistance(){
	int length_x = this->target.x - this->me.x;
	int length_y = this->target.y - this->me.y;
	float total = sqrt((float)length_x*length_x + (float)length_y*length_y);
	if (abs(total) < this->tolerance_distance)
			return 0;
	return (int)total;
}
void Robot::Drive(int distance, CanBotLL *c)
{
	c->drive(distance);
	//call low_level drive
}
void Robot::Turn(int arc, CanBotLL *c){
	c->turn(arc);
}

